
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  ,               sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  ,               sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  ,               sensorQuadEncoder)
#pragma config(Sensor, I2C_1,  claw_con1,      sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           Claw1,         tmotorVex393_HBridge, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port2,           Arm1,          tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           Arm2,          tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           Arm3,          tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           LeftWheel,     tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port6,           RightWheel,    tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           Arm4,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           Arm5,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           Arm6,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          Claw2,         tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(60)
#pragma userControlDuration(60)

#include "robotcchinasbsbdasb.c"   //Main competition background code...do not modify! caonimalaozijiugainidiceng
/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
  // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}
//!!!!!!!!!!!!!!!!!!!Never Write Anything Over Me To Protect Auto-Code!!!!!!!!!!!!!!!!!!!
//!!!!!!!!!!!!!!!!!!!Never Write Anything Over Me To Protect Auto-Code!!!!!!!!!!!!!!!!!!!
//!!!!!!!!!!!!!!!!!!!Never Write Anything Over Me To Protect Auto-Code!!!!!!!!!!!!!!!!!!!
/////////////////////////////////////////////////////////////////////////////////////////
//  Counter Struct
/////////////////////////////////////////////////////////////////////////////////////////
typedef struct {
	unsigned long Counter;
	unsigned long Limit;
}Counter;

void Counter_Reset(unsigned long limit,Counter &c){
	c.Counter = 0;
	c.Limit   = limit;
}

bool Counter_Run(bool condition,Counter &c){
	if(condition)	c.Counter ++;
	else 					c.Counter =0;

	return c.Counter >= c.Limit;
}
/////////////////////////////////////////////////////////////////////////////////////////
//  Controller Struct
/////////////////////////////////////////////////////////////////////////////////////////
typedef struct {
	float A,I,D;
	long DiedArea;
	long Sum,Last;
}Controller;

void Controller_Reset(float a,float i,float d,long diedArea,Controller &c){
	c.A = a;
	c.I = i;
	c.D = d;
	c.DiedArea = diedArea;
	c.Sum = c.Last = 0;
}

long Controller_Run(long error,Controller &c){
	if(abs(error)<c.DiedArea)return 0;
	long derror=error-c.Last;
	c.Last=error;
	c.Sum+=error;

	return (long)(c.A*(error + c.I*c.Sum + c.D*derror));
}
/////////////////////////////////////////////////////////////////////////////////////////
//  LowPassFilter Struct
/////////////////////////////////////////////////////////////////////////////////////////
typedef struct {
	short Forget;
	long Memory;
}LowPassFilter;

void LowPassFilter_Reset(short forget,LowPassFilter &c){
	c.Forget = forget;
	c.Memory = 0;
}

long LowPassFilter_Run(long value,LowPassFilter &c){
	c.Memory = ((100-c.Forget)*c.Memory + c.Forget*value)/100;
	return c.Memory;
}
/////////////////////////////////////////////////////////////////////////////////////////
//  Control Thread
/////////////////////////////////////////////////////////////////////////////////////////
//Define:
const char 	Left = 0,
						Right= 1,
						Arm  = 2,
						Claw = 3,
						OpenCircle     = 0,
						PositionControl= 1;

//Readonly:
long currentPosition[]={0,0,0,0};
long error					[]={0,0,0,0};
Controller 		P[4];
LowPassFilter F[4];

//ToBeWritten:
char Mode		 []={0,0,0,0};
long Position[]={0,0,0,0};
int  Speed	 []={0,0,0,0};

//Method:
//Get
void GetData(){
	currentPosition[Left]	=SensorValue[dgtl1];
	currentPosition[Right]=SensorValue[dgtl3];
	currentPosition[Arm]	=-SensorValue[dgtl5];
	currentPosition[Claw]	=getMotorEncoder(Claw1);
}
//Work
void LeftWork()	{motor[LeftWheel] = LowPassFilter_Run(Speed[Left] , F[Left] );}
void RightWork(){motor[RightWheel]= LowPassFilter_Run(Speed[Right], F[Right]);}
void ClawWork()	{motor[Claw1] = motor[Claw2] = LowPassFilter_Run(Speed[Claw], F[Claw]);}
void ArmWork(){
	  	motor[Arm1]
	  = motor[Arm2]
	  = motor[Arm3]
	  = motor[Arm4]
	  = motor[Arm5]
	  = motor[Arm6]
	  = LowPassFilter_Run(Speed[Arm],F[Arm]) ;
}
//Control
void LeftControl(){
	error[Left] = Position[Left] - currentPosition[Left];
	Speed[Left] = Controller_Run(error[Left], P[Left]);
}
void RightControl(){
	error[Right]= Position[Right]- currentPosition[Right];
	Speed[Right]= Controller_Run(error[Right],P[Right]);
}
void ArmControl(){
	error[Arm]  = Position[Arm]  - currentPosition[Arm];
	Speed[Arm]  = Controller_Run(error[Arm],  P[Arm]);
}
void ClawControl(){
	error[Claw] = Position[Claw] - currentPosition[Claw];
	Speed[Claw] = Controller_Run(error[Claw], P[Claw]);
}
//Do
task Control{
	Controller_Reset( 0.8, 0,  0, 0, P[Left] );
	Controller_Reset( 0.8, 0,  0, 0, P[Right]);
	Controller_Reset( 0.6, 0, 10, 5, P[Arm]	 );
	Controller_Reset( 0.8, 0,  5, 0, P[Claw] );

	LowPassFilter_Reset( 100, F[Left] );
	LowPassFilter_Reset( 100, F[Right]);
	LowPassFilter_Reset( 100, F[Arm]  );
	LowPassFilter_Reset( 10, F[Claw] );

	while(1){
		GetData();

		displayLCDPos(0,0);
  	displayNextLCDString("A:");
  	displayLCDPos(0,2);
  	displayNextLCDNumber(currentPosition[Arm] );
  	displayLCDPos(0,6);
  	displayNextLCDString("L:");
  	displayLCDPos(0,8);
  	displayNextLCDNumber(currentPosition[Left] );
  	displayLCDPos(1,0);
  	displayNextLCDString("C:");
  	displayLCDPos(1,2);
  	displayNextLCDNumber(currentPosition[Claw]);

		if			(Mode[Left] ==OpenCircle)			{/*Nothing*/		}
		else if	(Mode[Left] ==PositionControl){LeftControl();	}

		if			(Mode[Right]==OpenCircle)			{/*Nothing*/		}
		else if	(Mode[Right]==PositionControl){RightControl();}

		if			(Mode[Arm]  ==OpenCircle)			{/*Nothing*/		}
		else if	(Mode[Arm]  ==PositionControl){ArmControl();	}

		if			(Mode[Claw] ==OpenCircle)			{/*Nothing*/		}
		else if	(Mode[Claw] ==PositionControl){ClawControl();	}

		LeftWork	();
		RightWork	();
		ArmWork		();
		ClawWork	();

		wait1Msec(1);
	}
}
/////////////////////////////////////////////////////////////////////////////////////////
//  Autonomous
/////////////////////////////////////////////////////////////////////////////////////////
Counter C[2];
void ResetC0(unsigned long limit){Counter_Reset(limit,C[0]);}
void ResetC1(unsigned long limit){Counter_Reset(limit,C[1]);}
bool RunC0(bool condition){return Counter_Run(condition,C[0]);}
bool RunC1(bool condition){return Counter_Run(condition,C[1]);}

task autonomous{
	//initialize:------------------------------------
	Mode[Left] = OpenCircle;
	Mode[Right]= OpenCircle;
	Mode[Arm]  = PositionControl;
	Mode[Claw] = PositionControl;

	  SensorValue[dgtl1]
	= SensorValue[dgtl3]
	= SensorValue[dgtl5] = 0;
	resetMotorEncoder(Claw1);



	startTask(Control);
	//action1:---------------------------------------
	Speed[Left]=Speed[Right]= 0;
	Position[Arm] =  600;
	do{/*Nothing*/}while(currentPosition[Arm] <400);
  //action2:---------------------------------------
	Position[Claw]= 1400;
	do{/*Nothing*/}while(currentPosition[Claw]<900);
	//action3:---------------------------------------
	Position[Arm] = -20;
	ResetC0(200);
	ResetC1(200);
	do{/*Nothing*/}while(!RunC0(abs(error[Arm])<20)&&!RunC1(abs(error[Claw])<20));
	//action4:---------------------------------------
	wait(2);
	Speed[Left]=Speed[Right]=127;
	wait1Msec(1500);
	Speed[Left]=Speed[Right]=0;

	Mode[Arm]  = OpenCircle;
	Mode[Claw] = OpenCircle;

	Speed[Claw]=127;
	wait1Msec(800);
	Speed[Claw]=40;
	wait1Msec(200);

	Speed[Left]=Speed[Right]=-127;
	wait1Msec(400);
	Speed[Left]=Speed[Right]=0;

	Speed[Arm]=127;
	wait1Msec(800);
	Speed[Arm]=0;

	Speed[Left]=Speed[Right]=-127;
	wait1Msec(400);
	Speed[Left]=Speed[Right]=0;

	wait1Msec(500);

	Speed[Right]=-127;
	Speed[Left]=0;
	wait1Msec(800);

	Speed[Left]=Speed[Right]=-127;
	wait1Msec(400);

	Speed[Arm]=127;
	wait1Msec(200);
	Speed[Claw]=-127;
	wait1Msec(400);


	//final:---------------------------------------
	Mode[Left]=Mode[Right]=Mode[Arm]=Mode[Claw]=OpenCircle;
	Speed[Arm] = Speed[Claw]= Speed[Left]=Speed[Right]=0;
}




/////////////////////////////////////////////////////////////////////////////////////////
//  RemoteControl
/////////////////////////////////////////////////////////////////////////////////////////
task usercontrol(){
	//initialize:------------------------------------
	Mode[Left] = OpenCircle;
	Mode[Right]= OpenCircle;
	Mode[Arm]  = OpenCircle;
		  SensorValue[dgtl1]
	= SensorValue[dgtl3]
	= SensorValue[dgtl5] = 0;
	resetMotorEncoder(Claw1);
	startTask(Control);
	//operator:--------------------------------------
	while (true){
	  Speed[Left] = vexRT[Ch3];
	  Speed[Right]= vexRT[Ch2];

	  if(vexRT[Btn5U]&&!vexRT[Btn5D])     Speed[Arm]=-127;
	  else if(!vexRT[Btn5U]&&vexRT[Btn5D])Speed[Arm]= 127;
	  else Speed[Arm]= 0;

	  if(vexRT[Btn6U]&&!vexRT[Btn6D]){
	  	Mode[Claw]=OpenCircle;
	  	Speed[Claw]=127;
	  	Position[Claw]=currentPosition[Claw];
	  }else if(!vexRT[Btn6U]&&vexRT[Btn6D]){
	  	Mode[Claw]=OpenCircle;
	  	Speed[Claw]=-127;
	  	Position[Claw]=currentPosition[Claw];
	  }else Mode[Claw]=PositionControl;
	}
}
